/*
 * Copyright (C) 2015-2019 Alibaba Group Holding Limited
 */

#include "co_master_core.h"

#include <stdio.h>

#include "ulog/ulog.h"
#include "aos/kernel.h"
#include "canopen/co_io.h"
#include "canopen/canopen.h"
#include "co_master_config.h"
#include "co_master.h"
#include "board.h"

static aos_queue_t co_master_core_queue;

static uint8_t schedule_inited = 0;

static uint8_t *co_core_queue_buffer = NULL;

static uint8_t co_master_initd = 0;

static void user_pdo_rx(const pdo_rx_cb_para_t *rx_data, void *user_data)
{
    if (NULL != rx_data) {
        LOGD(CO_MASTER_LOG_TAG, "Handle user rcv (%d %d) PDO %d len %d data[0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x]",
            rx_data->port, rx_data->node, rx_data->idx, rx_data->len,
            rx_data->data[0], rx_data->data[1], rx_data->data[2], rx_data->data[3],
            rx_data->data[4], rx_data->data[5], rx_data->data[6], rx_data->data[7]);

        if (GPIO_OUT_RXPDO_IDX == rx_data->idx && NULL != user_data) {
            if(PORT_CAN_IO == rx_data->port && rx_data->len <= CO_IO_OUTPUT_DATA_SIZE) {
                co_io_table_list_t *list = get_io_node(rx_data->node);
                if(NULL != list) {
                    memset(list->input_actual_value, 0, sizeof(list->input_actual_value));
                    memcpy( list->input_actual_value, rx_data->data, rx_data->len);
                }
            }
            ((can_io_input_change_cb)user_data)(rx_data->port, rx_data->node, rx_data->len, rx_data->data);
        } else if (GPIO_OUTPUT_COFIRM_PDO_IDX == rx_data->idx) {
            if(PORT_CAN_IO == rx_data->port) {
                co_io_table_list_t *list = get_io_node(rx_data->node);

                if(NULL != list) {
                    memset(list->output_actual_value, 0, sizeof(list->output_actual_value));
                    memcpy( list->output_actual_value, rx_data->data, rx_data->len<CO_IO_OUTPUT_DATA_SIZE?rx_data->len:CO_IO_OUTPUT_DATA_SIZE);
                }
            }

        }
    }
}

static void user_components_state_change(const can_node_id_t *node, const nmt_state_t state, void *user_data)
{
    if (NULL != node && NULL != user_data) {
        can_io_state_change_cb p = (can_io_state_change_cb)user_data;
        LOGD(CO_MASTER_LOG_TAG, "Port %d Node (0x%x) State Change to %d", node->port, node->node, state);
        switch (state)
        {
        case nmt_state_pre_operational:
        {
            co_io_table_list_t *co_io_node = NULL;
            co_master_notify(co_master_core_sdo, sizeof(node->node), &node->node);
            co_io_node = get_io_node(node->node);
            if (NULL != co_io_node && io_board_lost_connection != co_io_node->state) {
                p(node->port, node->node, io_board_lost_connection);
            }
        }
        break;

        case nmt_state_operational:
            p(node->port, node->node, io_board_working);
            break;

        case nmt_state_upgrade:
            p(node->port, node->node, io_board_upgrading);
            break;

        case nmt_state_init:
            p(node->port, node->node, io_board_lost_connection);
            break;

        default:
            break;
        }

    }
}



static void co_master_routine(void *param)
{
    char *event_tmp = (char*)aos_malloc(sizeof(co_master_core_event_header_t) + CO_MASTER_CORE_EVENT_PARAM_MAX_LEN);

    if (NULL != event_tmp) {
        unsigned int rcv_size = 0;
        co_master_core_event_header_t *event_header = (co_master_core_event_header_t*)event_tmp;

        while (1) {
            if (0 == aos_queue_recv(&co_master_core_queue, AOS_WAIT_FOREVER, event_tmp, &rcv_size)) {
                switch (event_header->event)
                {
                case co_master_core_reset_all:
                    co_node_reset_all(PORT_CAN_IO);

                    break;

                case co_master_core_sdo:
                    if (event_header->length == sizeof(uint8_t)) {
                        uint8_t i = 0;
                        uint8_t *pnode = (uint8_t *)&event_tmp[sizeof(co_master_core_event_header_t)];

#if CO_MASTER_SKIP_OD_ACCESS
                        bool od_access_completely = true;
                        LOGI(CO_MASTER_LOG_TAG, "Skip SDO Access, directly going to operation");
#else
                        LOGI(CO_MASTER_LOG_TAG, "OD Access Event");
                        sdo_access_t *od_access = NULL;
                        can_node_id_t node_id = { PORT_CAN_IO, *pnode };

                        bool od_access_completely = false;

                        while (NULL != (od_access = foreach_od_table(&i, *pnode))) {
                            if (co_sdo_access_download == od_access->access) {
                                int32_t rc = co_sdo_write_od(&node_id, od_access->index, od_access->sub_index, od_access->data, od_access->len);

                                if (0 != rc) {
                                    LOGE(CO_MASTER_LOG_TAG, "OD Write on 0x%x %d fail, node %d", od_access->index, od_access->sub_index, *pnode, rc);
                                    break;
                                } else {
                                    LOGI(CO_MASTER_LOG_TAG, "OD Write on 0x%x %d OK node %d", od_access->index, od_access->sub_index, *pnode);
                                }
                            } else if (co_sdo_access_upload == od_access->access) {
                                LOGD(CO_MASTER_LOG_TAG, "Version OD Buffer %p %d", od_access->data, od_access->len);
                                int32_t rc = co_sdo_read_od(&node_id, od_access->index, od_access->sub_index, od_access->data, od_access->len);

                                if (0 > rc) {
                                    LOGE(CO_MASTER_LOG_TAG, "OD Read on 0x%x %d fail(%d), node %d", od_access->index, od_access->sub_index, rc, *pnode);
                                    break;
                                } else {
                                    LOGI(CO_MASTER_LOG_TAG, "OD Read on 0x%x %d OK(%d), node %d", od_access->index, od_access->sub_index, rc, *pnode);
                                    if(CO_OUTPUT_INDEX==od_access->index && CO_OUTPUT_SUB_INDEX_DEFAULT==od_access->sub_index) {
                                        co_io_table_list_t *io_node = get_io_node(node_id.node);
                                        if(NULL != io_node) {
                                            LOGI(CO_MASTER_LOG_TAG, "Sync Default output of node %d output len %d", node_id.node, od_access->len);
                                            memcpy(io_node->output_value, od_access->data, od_access->len<CO_IO_OUTPUT_DATA_SIZE?od_access->len:CO_IO_OUTPUT_DATA_SIZE);
                                        } else {
                                            LOGE(CO_MASTER_LOG_TAG, "Unknown node %d", node_id.node);
                                        }
                                    }
                                }    
                                
                            } else if (co_sdo_access_complete == od_access->access) {
                                od_access_completely = true;
                                break;
                            }
                            i++;
                        }
#endif
                        if (od_access_completely) {
                            /* make component enter into operation */
                            if (0 != co_node_enter_operation(PORT_CAN_IO, *pnode)) {
                                LOGE(CO_MASTER_LOG_TAG, "MAKE Node Enter Operation Fail", *pnode);
                            }
                        } else {
                            LOGE(CO_MASTER_LOG_TAG, "OD Access Pending @ %d", i);
                            /* we seem this as lost connection */
                            co_node_lost_connect(PORT_CAN_IO, *pnode);

                        }

                    } else {
                        LOGW(CO_MASTER_LOG_TAG, "invalid length of input(%d)", event_header->length);
                    }

                    break;

                default:

                    LOGE(CO_MASTER_LOG_TAG, "Unknown Event %d(%d)", event_header->event, rcv_size);

                    break;
                }
            }
        }

        aos_free(event_tmp);
        event_tmp = NULL;
    } else {
        LOGE(CO_MASTER_LOG_TAG, "allocate event tmp fail");
    }
}

int32_t co_master_core_init(void)
{
    int rc = -1;

    if (0 == schedule_inited) {
        const uint16_t queue_size = (sizeof(co_master_core_event_header_t) + CO_MASTER_CORE_EVENT_PARAM_MAX_LEN)*CO_MASTER_EVENT_QUEUE_SIZE;

        co_core_queue_buffer = (uint8_t*)aos_malloc(queue_size);
        if (NULL != co_core_queue_buffer) {
            if (0 == aos_queue_new(&co_master_core_queue, co_core_queue_buffer, queue_size,
                sizeof(co_master_core_event_header_t) + CO_MASTER_CORE_EVENT_PARAM_MAX_LEN)) {
                if (0 != aos_task_new("co_master", co_master_routine, NULL, CO_MASTER_CORE_STACK_SIZE)) {
                    LOGE(CO_MASTER_LOG_TAG, "create routine fail");
                } else {
                    schedule_inited = 1;
                    rc = 0;
                }
            } else {
                LOGE(CO_MASTER_LOG_TAG, "create queue fail");
            }

            if (0 != rc) {
                aos_free(co_core_queue_buffer);
                co_core_queue_buffer = NULL;
            }
        }
    }
    return rc;
}

int32_t co_master_notify(const co_master_core_event_t type, const uint8_t len, const void *param)
{
    int32_t rc = -1;
    if (1 == schedule_inited && len <= CO_MASTER_CORE_EVENT_PARAM_MAX_LEN) {
        char tmpbuf[CO_MASTER_CORE_EVENT_PARAM_MAX_LEN + sizeof(co_master_core_event_header_t)];
        co_master_core_event_header_t *p = tmpbuf;

        p->event = type;
        p->length = len;

        if (NULL != param && len > 0) {
            memcpy(&tmpbuf[sizeof(co_master_core_event_header_t)], param, len);
        }
        rc = aos_queue_send(&co_master_core_queue, tmpbuf, sizeof(co_master_core_event_header_t) + len);
    }

    return rc;
}

int32_t co_master_init(const uint8_t can_port, const uint8_t *node_list, const uint8_t node_cnt, can_io_state_change_cb state_change_cb, can_io_input_change_cb input_change_cb)
{
    int32_t rc = -1;

    if (NULL != node_list && node_cnt > 0 && 0 == co_master_initd) {

        if (0 == canopen_init(can_port, CANOPEN_MASTER, NULL)) {
            LOGI(CO_MASTER_LOG_TAG, "start canopen");

            if (node_install(can_port, node_cnt, node_list, user_components_state_change, state_change_cb) != 0) {
                LOG("Node Install fail");
            } else {
                rc = 0;
                LOGI(CO_MASTER_LOG_TAG, "register pdo");
                uint8_t i = 0;
                for (; i < node_cnt; i++) {

                    pdo_register_cb(can_port, node_list[i], user_pdo_rx, input_change_cb);

                    if (0 != sdo_install(can_port, node_list[i], co_sdo_role_client, NULL, 0)) {
                        LOGE(CO_MASTER_LOG_TAG, "install sdo failed");
                    }
                }

                aos_msleep(100);

            }
        } else {
            LOGE(CO_MASTER_LOG_TAG, "start canopen fail");
        }
        co_master_initd = 1;
    }

    return rc;
}

int32_t gpio_set_output(const uint8_t can_port, const uint8_t node, const uint8_t gpio_number, const bool set)
{
    int32_t rc = -1;
    co_io_table_list_t *io_table_node = get_io_node(node);

    if (NULL != io_table_node && gpio_number < io_table_node->output_cnt) {
        if (set) {
            io_table_node->output_value[gpio_number / 8] |= (1 << (gpio_number % 8));
        } else {
            io_table_node->output_value[gpio_number / 8] &= ~(1 << (gpio_number % 8));
        }

        pdo_tx_set_t pdo_tx_data = { io_table_node->output_cnt / 8 + ((io_table_node->output_cnt % 8 == 0) ? 0 : 1), 0, io_table_node->output_value };
        rc = pdo_set(can_port, node, GPIO_OUT_RXPDO_IDX, &pdo_tx_data, true);
    }

    return rc;
}

